

#include <FracturePiece.h>

#include <Voxelization.h>

#include "GeometryUtils.h"

#include <assert.h>



void FracturePiece::buildVertexNeighbourhoods()
{
	// Remap triangle indices to local ones.
	assert(m_vertices.size() == 0);
	
	map<int, int> latticeToLocalMap;
	
	for (int ti=0; ti<m_triangles.size(); ++ti)
	{
		const Geometry::Triangle& triangle = m_triangles[ti];
		for (int a=0; a<3; ++a)
		{
			int latticeIndex = triangle.m_vertex[a];
			if ( latticeToLocalMap.count(latticeIndex) == 0 )
			{
				int localIndex = m_vertices.size();
				latticeToLocalMap.insert(map<int, int>::value_type(latticeIndex, localIndex));
				m_vertices.push_back(latticeIndex);
			}
		}
	} 
	
	for (int ti=0; ti<m_triangles.size(); ++ti)
	{
		Geometry::Triangle& triangle = m_triangles[ti];
		for (int a=0; a<3; ++a)
		{
			int latticeIndex = triangle.m_vertex[a];
			int localIndex = latticeToLocalMap[latticeIndex];
			triangle.m_vertex[a] = localIndex;
		}
	}
	
	// Build vertex neighbourhoods
	for (int vi=0; vi<m_vertices.size(); ++vi)
	{
		VertexNeighbourhood neighbourhood(m_vertices[vi]);
		m_vertexNeighbourhoods.push_back(neighbourhood);
	}
	
	for (int ti=0; ti<m_triangles.size(); ++ti)
	{
		const Geometry::Triangle& triangle = m_triangles[ti];
		for (int a=0; a<3; ++a)
		{
			int localIndex = triangle.m_vertex[a];
			VertexNeighbourhood& neighbourhood = m_vertexNeighbourhoods[localIndex]; 
			neighbourhood.addTriangle(ti, triangle.m_vertex[0], triangle.m_vertex[1], triangle.m_vertex[2]);
		}
	}
}


void FracturePiece::smooth(Voxelization& voxelization)
{
	// Laplacian smoothing
	
	// Work through 1-ring neighbourhoods
	for (int vi=0; vi<m_vertices.size(); ++vi)
	{
		int latticeIndex = m_vertices[vi];
		
		float weight = 0.5f;
		
		// Do not disturb vertices on original boundary (near mesh) as much
		if ( voxelization.m_boundaryVertices.count(latticeIndex) != 0 ) weight = 0.05f;

		Vec4 sumQ;
		sumQ.setZero3();
		
		const VertexNeighbourhood& neighbourhood = m_vertexNeighbourhoods[vi];
		float invN = 1.f/float(neighbourhood.m_neighbouringVertices.size());
		
		for (int n=0; n<neighbourhood.m_neighbouringVertices.size(); ++n)
		{
			int neighbourLocalIndex = neighbourhood.m_neighbouringVertices[n];
			int neighbourLatticeIndex = m_vertices[neighbourLocalIndex];
			
			int viq, vjq, vkq;
			voxelization._vertexCoords(neighbourLatticeIndex, viq, vjq, vkq);
			Vec4 Q = voxelization._latticeVertex(viq, vjq, vkq);
			sumQ += Q;
		}

		if (0)
		{
			// Laplacian
			Vec4 P = sumQ*invN;
			voxelization._setLatticeVertex(latticeIndex, P);
		}
		else
		{
			// Centroid-preserving Laplacian
			int vip, vjp, vkp;
			voxelization._vertexCoords(latticeIndex, vip, vjp, vkp);
			Vec4 P = voxelization._latticeVertex(vip, vjp, vkp);
			
			Vec4 delta =  sumQ*invN - P;
			
			P += weight * delta;
			voxelization._setLatticeVertex(latticeIndex, P);
			
			for (int n=0; n<neighbourhood.m_neighbouringVertices.size(); ++n)
			{
				int neighbourLocalIndex = neighbourhood.m_neighbouringVertices[n];
				int neighbourLatticeIndex = m_vertices[neighbourLocalIndex];
				
				int viq, vjq, vkq;
				voxelization._vertexCoords(neighbourLatticeIndex, viq, vjq, vkq);
				Vec4 Q = voxelization._latticeVertex(viq, vjq, vkq);
				
				Q += (-weight*invN) * delta;
				voxelization._setLatticeVertex(neighbourLatticeIndex, Q);
			}			
		}
	}
}


void FracturePiece::finalizeGeometry(Voxelization& voxelization)
{
	// FILL THIS!
	Geometry& geometry = m_geometry;
	
	Vec4 zeroVec; zeroVec.setZero4();
	m_geometry.m_vertices.resize(m_vertices.size());
	m_geometry.m_vertexNormals.resize(m_vertices.size(), zeroVec);
	m_geometry.m_boundaryVertex.resize(m_vertices.size(), false);
	
	Vec4 uvDefault(0.5f, 0.5f, 0.f);
	m_geometry.m_UVs.resize(m_vertices.size(), uvDefault);
	
	// First set normals by averaging triangle normals, and set UVs to a default
	for (int localIndex=0; localIndex<m_vertices.size(); ++localIndex)
	{
		int latticeIndex = m_vertices[localIndex];
		
		int vi, vj, vk;
		voxelization._vertexCoords(latticeIndex, vi, vj, vk);
		Vec4 P = voxelization._latticeVertex(vi, vj, vk);
		
		m_geometry.m_vertices[localIndex] = P;
	}
	
	for (int ti=0; ti<m_triangles.size(); ++ti)
	{
		Geometry::Triangle& triangle = m_triangles[ti];
		for (int a=0; a<3; ++a)
		{
			triangle.m_vertexNormal[a] = triangle.m_vertex[a];
			triangle.m_uvs[a] = triangle.m_vertex[a];
		}
		geometry.m_triangles.push_back(triangle);
	}
	
	for (int ti=0; ti<m_triangles.size(); ++ti)
	{
		Geometry::Triangle& triangle = m_triangles[ti];
		
		const Vec4& A = m_geometry.m_vertices[triangle.m_vertex[0]];
		const Vec4& B = m_geometry.m_vertices[triangle.m_vertex[1]];
		const Vec4& C = m_geometry.m_vertices[triangle.m_vertex[2]];
		
		Vec4 N = (B-A) % (C-A); // Assumed winding may be wrong, will fix shortly if so
		N.normalizeIfNotZero();
		
		// Weight normals by angle subtended at triangle vertex.
		Vec4 BA = B-A;
		Vec4 CA = C-A;
		float angleA = acos(BA * CA / (BA.length3() * CA.length3()));
		
		Vec4 AB = A-B;
		Vec4 CB = C-B;
		float angleB = acos(AB * CB / (AB.length3() * CB.length3()));
		
		Vec4 AC = A-C;
		Vec4 BC = B-C;
		float angleC = acos(AC * BC / (AC.length3() * BC.length3()));
		
		m_geometry.m_vertexNormals[triangle.m_vertex[0]] += angleA * N;
		m_geometry.m_vertexNormals[triangle.m_vertex[1]] += angleB * N;
		m_geometry.m_vertexNormals[triangle.m_vertex[2]] += angleC * N;
	}
	
	for (int vi=0; vi<m_geometry.m_vertexNormals.size(); ++vi)
	{
		Vec4& N = m_geometry.m_vertexNormals[vi];
		N.normalizeIfNotZero();
	}
	
	// Now deal with boundary ones - use majority vote to decide whether to winding flip
	float flip = 0.0f;
	for (int localIndex=0; localIndex<m_vertices.size(); ++localIndex)
	{
		int latticeIndex = m_vertices[localIndex];
		if (voxelization.m_boundaryVertices.count(latticeIndex) == 0) continue;
		
		Vec4 N = voxelization.m_boundaryNormals[latticeIndex];
		Vec4 UV = voxelization.m_boundaryUVs[latticeIndex];
		
		Vec4 fiducialN = m_geometry.m_vertexNormals[localIndex];
		flip += N * fiducialN;
		
		m_geometry.m_vertexNormals[localIndex] = N;
		m_geometry.m_UVs[localIndex] = UV;
		m_geometry.m_boundaryVertex[localIndex] = true;
	}
	
	// May need to flip non-boundary normals if our assumed winding was wrong (i.e. conflicts with mesh normals)
	if (flip<0.0f)
	{
		for (int localIndex=0; localIndex<m_vertices.size(); ++localIndex)
		{
			int latticeIndex = m_vertices[localIndex];
			if (voxelization.m_boundaryVertices.count(latticeIndex) != 0) continue;
			
			Vec4& N = m_geometry.m_vertexNormals[localIndex];
			N *= -1.f;
		}
	}
}


// Search via BFS from start vertex until can search no more (move only into unseen vertex neighbours), recording vertex indices.
static void _growConnected(int startVertex, set<int>& connectedVertices, const vector<VertexNeighbourhood>& vertexNeighbourhoods)
{
	std::queue<int> Q;
	Q.push(startVertex);
	
	while (Q.size() != 0)
	{
		int nextVertexIndex = Q.front();
		Q.pop();
		
		if (connectedVertices.count(nextVertexIndex) != 0) continue; 
		connectedVertices.insert(nextVertexIndex);
		
		const VertexNeighbourhood& neighbourhood = vertexNeighbourhoods[nextVertexIndex];
		for (int n=0; n<neighbourhood.m_neighbouringVertices.size(); ++n)
		{
			int nbVertex = neighbourhood.m_neighbouringVertices[n];
			Q.push(nbVertex);
		}
	}
}


void FracturePiece::disconnect(vector<Geometry>& finalGeometries)
{
	vector<bool> vertexHandled;
	vertexHandled.resize(m_geometry.m_vertices.size(), false);
	
	int pieceCount=0;
	
	while(1)
	{
		// Find first unseen vertex (or finished if all seen)
		int firstUnhandled = -1;
		for (int vi=0; vi<vertexHandled.size(); ++vi)
		{
			if (!vertexHandled[vi])
			{
				firstUnhandled = vi;
				break;
			}
		}
		if (firstUnhandled<0) break;
		
		pieceCount++;
		
		// collect all vertices connected to that vertex
		set<int> connectedVertices;
		_growConnected(firstUnhandled, connectedVertices, m_vertexNeighbourhoods);
		
		// Now we have to remap vertex/normal/UV indices from the fracture piece geometry into our new sub-geometry
		vector<int> oldVertexIndex_To_NewVertexIndex;
		vector<int> oldVertexNormalIndex_To_NewVertexNormalIndex;
		vector<int> oldUVIndex_To_NewUVIndex;
		vector<int> oldTriangleIndex_To_NewTriangleIndex;
		
		oldVertexIndex_To_NewVertexIndex.resize(m_geometry.m_vertices.size(), -1);
		oldVertexNormalIndex_To_NewVertexNormalIndex.resize(m_geometry.m_vertexNormals.size(), -1);
		oldUVIndex_To_NewUVIndex.resize(m_geometry.m_UVs.size(), -1);
		oldTriangleIndex_To_NewTriangleIndex.resize(m_geometry.m_triangles.size(), -1);
		
		int newVertexIndex = 0;
		int newVertexNormalIndex = 0;
		int newUVIndex = 0;
		int newTriangleIndex = 0;
		
		printf("     separating disconnected piece %d ... ", pieceCount);
		
		for( set<int>::iterator iter = connectedVertices.begin(); iter != connectedVertices.end(); iter++)
		{
			int vertexIndex = *iter;
			vertexHandled[vertexIndex] = true;
			const VertexNeighbourhood& neighbourhood = m_vertexNeighbourhoods[vertexIndex];
			
			for (int ti=0; ti<neighbourhood.m_parentTriangles.size(); ++ti)
			{
				int oldTriangleIndex = neighbourhood.m_parentTriangles[ti];
				Geometry::Triangle& oldTriangle = m_geometry.m_triangles[oldTriangleIndex];
				
				if (oldTriangleIndex_To_NewTriangleIndex[oldTriangleIndex]==-1)
				{
					oldTriangleIndex_To_NewTriangleIndex[oldTriangleIndex] = newTriangleIndex;
					newTriangleIndex++;
				}
				
				for (int a=0; a<3; ++a)
				{
					int oldVertexIndex = oldTriangle.m_vertex[a];
					if (oldVertexIndex_To_NewVertexIndex[oldVertexIndex]==-1)
					{
						oldVertexIndex_To_NewVertexIndex[oldVertexIndex] = newVertexIndex;
						newVertexIndex++;
					}
					
					int oldVertexNormalIndex = oldTriangle.m_vertexNormal[a];
					if (oldVertexNormalIndex_To_NewVertexNormalIndex[oldVertexNormalIndex]==-1)
					{
						oldVertexNormalIndex_To_NewVertexNormalIndex[oldVertexNormalIndex] = newVertexNormalIndex;
						newVertexNormalIndex++;
					}
					
					int oldUVIndex = oldTriangle.m_uvs[a];
					if (oldUVIndex_To_NewUVIndex[oldUVIndex]==-1)
					{
						oldUVIndex_To_NewUVIndex[oldUVIndex] = newUVIndex;
						newUVIndex++;
					}
				}
			}
		}
		
		printf("of %d vertices\n", newVertexIndex);
		
		// Construct new geometry
		Geometry newGeometry;
		newGeometry.m_vertices.resize(newVertexIndex);
		newGeometry.m_vertexNormals.resize(newVertexNormalIndex);
		newGeometry.m_UVs.resize(newUVIndex);
		newGeometry.m_triangles.resize(newTriangleIndex);
		newGeometry.m_boundaryVertex.resize(newVertexIndex);
		
		for (int oldVertexIndex=0; oldVertexIndex<m_geometry.m_vertices.size(); ++oldVertexIndex)
		{
			const Vec4& P = m_geometry.m_vertices[oldVertexIndex];
			bool boundaryVertex = m_geometry.m_boundaryVertex[oldVertexIndex];
			int newVertexIndex = oldVertexIndex_To_NewVertexIndex[oldVertexIndex];
			if (newVertexIndex>-1)
			{
				newGeometry.m_vertices[newVertexIndex] = P;
				newGeometry.m_boundaryVertex[newVertexIndex] = boundaryVertex;
			}
		}
		
		for (int oldVertexNormalIndex=0; oldVertexNormalIndex<m_geometry.m_vertexNormals.size(); ++oldVertexNormalIndex)
		{
			const Vec4& N = m_geometry.m_vertexNormals[oldVertexNormalIndex];
			int newVertexNormalIndex = oldVertexNormalIndex_To_NewVertexNormalIndex[oldVertexNormalIndex];
			if (newVertexNormalIndex>-1)
			{
				newGeometry.m_vertexNormals[newVertexNormalIndex] = N;
			}
		}
		
		for (int oldUVIndex=0; oldUVIndex<m_geometry.m_UVs.size(); ++oldUVIndex)
		{
			const Vec4& UV = m_geometry.m_UVs[oldUVIndex];
			int newUVIndex = oldUVIndex_To_NewUVIndex[oldUVIndex];
			if (newUVIndex>-1)
			{
				newGeometry.m_UVs[newUVIndex] = UV;
			}
		}
		
		for (int oldTriangleIndex=0; oldTriangleIndex<m_geometry.m_triangles.size(); ++oldTriangleIndex)
		{
			Geometry::Triangle& oldTriangle = m_geometry.m_triangles[oldTriangleIndex];
			int newTriangleIndex = oldTriangleIndex_To_NewTriangleIndex[oldTriangleIndex];
			if (newTriangleIndex>-1)
			{
				Geometry::Triangle& newTriangle = newGeometry.m_triangles[newTriangleIndex];
				for (int a=0; a<3; ++a)
				{
					newTriangle.m_vertex[a] = oldVertexIndex_To_NewVertexIndex[oldTriangle.m_vertex[a]];
					newTriangle.m_vertexNormal[a] = oldVertexNormalIndex_To_NewVertexNormalIndex[oldTriangle.m_vertexNormal[a]];
					newTriangle.m_uvs[a] = oldUVIndex_To_NewUVIndex[oldTriangle.m_uvs[a]];
				}
			}
		}
		
		finalGeometries.push_back(newGeometry);
	}
}



/* static */ void FracturePiece::repairInterfaces(Geometry& geometry)
{
	// Generate new UVs and normals along edges between boundary and non-boundary triangles, to remove UV artifacts.
	
	// Identify triangles on interface between mesh boundary vertices and non-boundary vertices
	set<int> interfaceTriangles;
	
	for (int ti=0; ti<geometry.m_triangles.size(); ++ti)
	{
		Geometry::Triangle& triangle = geometry.m_triangles[ti];
		int hasBoundaryVertex = false;
		int hasNonBoundaryVertex = false;
		
		for (int a=0; a<3; ++a)
		{
			if (geometry.m_boundaryVertex[triangle.m_vertex[a]]) hasNonBoundaryVertex = true;
			else hasBoundaryVertex = true;
		}
		
		if (hasBoundaryVertex && hasNonBoundaryVertex && interfaceTriangles.count(ti)==0) interfaceTriangles.insert(ti);
	}
	
	// Build vertex neighbourhood (needed to generate smooth interface normals)
	vector<VertexNeighbourhood> vertexNeighbourhoods; 
	for (int localIndex=0; localIndex<geometry.m_vertices.size(); ++localIndex)
	{
		VertexNeighbourhood neighbourhood(localIndex);
		vertexNeighbourhoods.push_back(neighbourhood);
	}
	for (int ti=0; ti<geometry.m_triangles.size(); ++ti)
	{
		const Geometry::Triangle& triangle = geometry.m_triangles[ti];
		for (int a=0; a<3; ++a)
		{
			int localIndex = triangle.m_vertex[a];
			VertexNeighbourhood& neighbourhood = vertexNeighbourhoods[localIndex]; 
			neighbourhood.addTriangle(ti, triangle.m_vertex[0], triangle.m_vertex[1], triangle.m_vertex[2]);
		}
	}
	
	// Make a new UV and normal for the interface triangle boundary vertices
	int interiorUVIndex = geometry.m_UVs.size();
	geometry.m_UVs.push_back(Vec4(0.5, 0.5, 0.f));
	
	for( set<int>::iterator iter = interfaceTriangles.begin(); iter != interfaceTriangles.end(); iter++)
	{
		int triIndex = *iter;
		Geometry::Triangle& triangle = geometry.m_triangles[triIndex];
		
		// Set interface vertex normal in the interface triangle to the average of the adjacent interface triangles
		for (int a=0; a<3; ++a)
		{
			if (geometry.m_boundaryVertex[triangle.m_vertex[a]])
			{
				VertexNeighbourhood& neighbourhood = vertexNeighbourhoods[triangle.m_vertex[a]];
				
				Vec4 Navg;
				Navg.setZero3();
				
				for (int pt=0; pt<neighbourhood.m_parentTriangles.size(); ++pt)
				{
					int parentTriangleIndex = neighbourhood.m_parentTriangles[pt];
					if (interfaceTriangles.count(parentTriangleIndex)==0) continue;
					
					Geometry::Triangle& parentTriangle = geometry.m_triangles[parentTriangleIndex];
					
					const Vec4& A = geometry.m_vertices[parentTriangle.m_vertex[0]];
					const Vec4& B = geometry.m_vertices[parentTriangle.m_vertex[1]];
					const Vec4& C = geometry.m_vertices[parentTriangle.m_vertex[2]];
					const Vec4& NA = geometry.m_vertexNormals[parentTriangle.m_vertexNormal[0]];
					const Vec4& NB = geometry.m_vertexNormals[parentTriangle.m_vertexNormal[1]];
					const Vec4& NC = geometry.m_vertexNormals[parentTriangle.m_vertexNormal[2]];
					
					Vec4 N = (B-A) % (C-A);
					N.normalizeIfNotZero();
					Navg += N;
					float flip = NA*N + NB*N + NC*N;
					if (flip<0.0f) N *= -1.f;
				}
				
				Navg.normalizeIfNotZero();
				
				triangle.m_vertexNormal[a] = geometry.m_vertexNormals.size();
				triangle.m_uvs[a] = interiorUVIndex;
				geometry.m_vertexNormals.push_back(Navg);
			}
		}
	}
}

